#include"vld_imu_slam/utility.h"

namespace vld_imu_slam{

sensor_msgs::Imu imuConverter2Lidar(const sensor_msgs::Imu& imu_in, const Config::Ptr p_config){
    sensor_msgs::Imu imu_out = imu_in;
    // rotate acceleration
    Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
    acc = p_config->ext_imu_lidar_rot_.inverse() * acc;
    imu_out.linear_acceleration.x = acc.x();
    imu_out.linear_acceleration.y = acc.y();
    imu_out.linear_acceleration.z = acc.z();
    // rotate gyroscope
    Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
    gyr = p_config->ext_imu_lidar_rot_.inverse() * gyr;
    imu_out.angular_velocity.x = gyr.x();
    imu_out.angular_velocity.y = gyr.y();
    imu_out.angular_velocity.z = gyr.z();

    return imu_out;
}

Eigen::Matrix4d SE3toEigenM(const SE3 &pose){
    Eigen::Matrix4d T_w = Eigen::Matrix4d::Identity();
    T_w.block<3, 3>(0, 0) = pose.rotationMatrix();
    T_w.block<3, 1>(0, 3) = pose.translation();
    return T_w;
}

void SE3toRt(const SE3 &pose,double* var){
    Eigen::Quaterniond q(pose.rotationMatrix());
    Eigen::Vector3d trans(pose.translation());
    var[0] = q.x();
    var[1] = q.y();
    var[2] = q.z();
    var[3] = q.w();
    var[4] = trans[0];
    var[5] = trans[1];
    var[6] = trans[2];
}

SE3 Rt2SE3(double* var){
    return SE3(Eigen::Quaterniond(var[3],var[0],var[1],var[2]),Eigen::Vector3d(var[4],var[5],var[6]));
}

double compute_inlier_residual_threshold(std::vector<double> residuals, double ratio)
{
    std::set<double> dis_vec; // 升序
    for (size_t i = 0; i < (size_t)(residuals.size() / 3); i++)
    {
        dis_vec.insert(fabs(residuals[3 * i + 0]) + fabs(residuals[3 * i + 1]) + fabs(residuals[3 * i + 2]));
    }

    return *(std::next(dis_vec.begin(), (int)((ratio)*dis_vec.size())));
}

Eigen::Matrix<double, 3, 1> pcl_pt_to_eigend(PointType &pt)
{
    return Eigen::Matrix<double, 3, 1>(pt.x, pt.y, pt.z);
}

void pcDownSample(pcl::PointCloud<PointType>::Ptr pc, double size){
    pcl::VoxelGrid<PointType> dw;
    dw.setInputCloud(pc);
    dw.setLeafSize(size,size,size);
    dw.filter(*pc);
}

sensor_msgs::PointCloud2 publishCloud(ros::Publisher& thisPub, pcl::PointCloud<PointType>::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
{
    sensor_msgs::PointCloud2 tempCloud;
    pcl::toROSMsg(*thisCloud, tempCloud);
    tempCloud.header.stamp = thisStamp;
    tempCloud.header.frame_id = thisFrame;
    if (thisPub.getNumSubscribers() != 0)
        thisPub.publish(tempCloud);
    return tempCloud;
}

void cout_array(double* var,int size){
    cout<<"var ";
    for(int i = 0; i<size; i++){
        cout<<var[i]<<" ";
    }
    cout<<endl;
}

void anti_symmetric(Eigen::Vector3d const &_v, Eigen::Matrix3d &_m)
{
    _m(0, 0) = 0.0; _m(0, 1) = -_v.z(); _m(0, 2) = _v.y();
    _m(1, 0) = _v.z();_m(1, 1) = 0.0;_m(1, 2) = -_v.x();
    _m(2, 0) = -_v.y();_m(2, 1) = _v.x();_m(2, 2) = 0.0;
}

void tranformPoint(const PointType p_in,PointType & p_out, Eigen::Matrix4d T){
    Eigen::Vector4d p_v(p_in.x,p_in.y,p_in.z,1);
    p_v = T*p_v;
    p_out.x = p_v[0];
    p_out.y = p_v[1];
    p_out.z = p_v[2];
}

double pointDistance(PointType p)
{
    return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}

}
